#include "maptransformer.h"

MapTransformer::MapTransformer(QObject *parent) : QThread(parent)
{
    tolerance_distance = 0.4;
    allocate_method = "MSTC";
    MST_shape = "DINIC";
    useROSPlanner = false;
    coverAndReturn = true;
    coverage_action =true;

    GA_max_iter = 200000;
    unchanged_iter = 2000;
    hp_max_iter = 5000;
}

void MapTransformer::setToleranceDistance(double toleranceDistance)
{
    tolerance_distance = toleranceDistance;
}

void MapTransformer::setRobotNum(int robotNum)
{
    robot_num = robotNum;
    robot_pos.clear();
    robot_pos.resize(robot_num);
}

void MapTransformer::setAllocateMethod(string allocateMethod)
{
    allocate_method =  allocateMethod;
}

void MapTransformer::setMstShape(string MSTShape)
{
    MST_shape = MSTShape;
}

void MapTransformer::setuseROSPlanner(bool useROSPlanner)
{
    this->useROSPlanner = useROSPlanner;
}

void MapTransformer::setMapScale(int mapScale)
{
    map_scale = mapScale;
}

void MapTransformer::setCoverageAction(bool coverageAction)
{
    this->coverage_action = coverageAction;
}

void MapTransformer::setGAMaxIter(int num)
{
    GA_max_iter = num;
}

void MapTransformer::setUnchangedIter(int num)
{
    unchanged_iter = num;
}

void MapTransformer::setHpMaxiter(int num)
{
    hp_max_iter = num;
}

void MapTransformer::setMap(const nav_msgs::OccupancyGrid &msg)
{
    //Update the map member variable
    map.data = msg.data;
    map.info = msg.info;
    map.header = msg.header;

    std::cout << "Receieved the map" << std::endl;
    cout << "Map's height and width: " << map.info.height << " " << map.info.width << endl;

    // Fill the coverage map
    // map's resolution = 0.05, so the scale is 20
    coverage_map.header = map.header;
    coverage_map.info.origin = map.info.origin;
    coverage_map.info.resolution = map.info.resolution * map_scale;
    coverage_map.info.width = (map.info.resolution * map.info.width) / coverage_map.info.resolution;
    coverage_map.info.height = (map.info.resolution * map.info.height) / coverage_map.info.resolution;
    coverage_map.data.resize(coverage_map.info.width * coverage_map.info.height);

    cout << "Map res and scale: " << map.info.resolution << " " << map_scale << endl;
    cout << "Coverage map's height and width: " << coverage_map.info.height << " " << coverage_map.info.width << endl;
    cout << "Coverage map resolution is : " << coverage_map.info.resolution << endl;
    // covert map to coverage map
    int scale = coverage_map.info.resolution / map.info.resolution;
    for(int row = 0; row < coverage_map.info.height; ++row){
        for(int col = 0; col < coverage_map.info.width; ++col){
            int sx = row * scale, sy = col * scale;
            bool valid = true;
            for(int i = sx; i < sx + scale; ++i){
                if(!valid)  break;
                for(int j = sy; j < sy + scale; ++j){
                    if(map.data[i * map.info.width + j] != 0){
                        valid = false;
                        break;
                    }
                }
            }
            coverage_map.data[row * coverage_map.info.width + col] = valid ? 1 : 0;
        }
    }


    cmw = coverage_map.info.width;  cmh = coverage_map.info.height;  cmres = coverage_map.info.resolution;
}

bool MapTransformer::setInitPose(vector<geometry_msgs::PoseWithCovarianceStamped> &vecInitPose)
{
    if(vecInitPose.size() < robot_num)
    {
        return false;
    }

    for(int i =0; i < robot_num; i++)
    {
        robot_pos[i].first = vecInitPose[i].pose.pose.position.x;
        robot_pos[i].second = vecInitPose[i].pose.pose.position.y;
    }

    return true;
}



bool MapTransformer::createPaths(vector<geometry_msgs::PoseWithCovarianceStamped> &vecInitPose)
{
    bool succ = setInitPose(vecInitPose);
    if(!succ)
    {
        return false;
    }

    //this->start();
    //this->run();
    Map.resize(cmh / 2, vector<int>(cmw / 2, 0));
    Region.resize(cmh, vector<int>(cmw, 0));

    // 去掉地图上的孤岛 一次BFS即可
    eliminateIslands();
    showMapAndRegionInf();


    if(!createPaths())
    {
        return false;
    }

    return true;
}

void MapTransformer::showMapAndRegionInf()
{
    // std::fstream map_data_file;
    // map_data_file.open("/home/courierlo/test_data/map.txt", ios::in | ios::out | ios::app);
    // map_data_file << "--------------------------------------------\n";
    // map_data_file << "Map: " << cmh / 2 << " " << cmw / 2 << "\n";
    for(int i = 0; i < cmh / 2; ++i){
        for(int j = 0; j < cmw / 2; ++j){
            if(coverage_map.data[(2 * i) * cmw + (2 * j)] && coverage_map.data[(2 * i) * cmw + (2 * j + 1)] &&
                coverage_map.data[(2 * i + 1) * cmw + (2 * j)] && coverage_map.data[(2 * i + 1) * cmw + (2 * j + 1)]){
                Map[i][j] = 1;
            }else{
                Map[i][j] = 0;
            }
            // map_data_file << Map[i][j];
        }
        // map_data_file << "\n";
    }

    for (int i = 0; i < Map.size(); ++i) {
        for (int j = 0; j < Map[0].size(); ++j) {
            Region[2 * i][2 * j] = Map[i][j];
            Region[2 * i][2 * j + 1] = Map[i][j];
            Region[2 * i + 1][2 * j] = Map[i][j];
            Region[2 * i + 1][2 * j + 1] = Map[i][j];
        }
    }

    // map_data_file.close();
}

void MapTransformer::eliminateIslands()
{
    int dir[4][2] = { { 0, 1 }, { 0, -1 }, { -1, 0 }, { 1, 0 } };
    vector<vector<bool> > vis(coverage_map.info.width, vector<bool>(coverage_map.info.height, false));
    queue<pair<int, int> > que;
    int sx = (robot_pos[0].first - coverage_map.info.origin.position.x) / coverage_map.info.resolution;
    int sy = (robot_pos[0].second - coverage_map.info.origin.position.y) / coverage_map.info.resolution;
    que.push({ sx, sy });
    vis[sx][sy] = true;

    while(!que.empty()){
        pair<int, int> p = que.front();  que.pop();
        coverage_map.data[p.second * coverage_map.info.width + p.first] = 2;
        for(int i = 0; i < 4; ++i){
            int dx = p.first + dir[i][0], dy = p.second + dir[i][1];
            if(dx >= 0 && dy >= 0 && dx < coverage_map.info.width && dy < coverage_map.info.height && coverage_map.data[dy * coverage_map.info.width + dx] && !vis[dx][dy]){
                que.push({ dx, dy });
                vis[dx][dy] = true;
            }
        }
    }

    for(int i = 0; i < coverage_map.data.size(); ++i){
        coverage_map.data[i] = coverage_map.data[i] == 2 ? 1 : 0;
    }
}

void MapTransformer::sendOneGoal(int id, int step, MoveBaseClient &ac)
{
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x = paths[id].poses[step].pose.position.x;
    goal.target_pose.pose.position.y = paths[id].poses[step].pose.position.y;
    goal.target_pose.pose.orientation.w = paths[id].poses[step].pose.orientation.w;
    goal.target_pose.pose.orientation.z = paths[id].poses[step].pose.orientation.z;

    string tips = "Sending goal. No." + std::to_string(id + 1) + "'s robot's " + std::to_string(step) + " step";
    //ROS_INFO("%s", tips);
    cout << "\033[34m" << tips << "\n";

    ac.sendGoal(goal);
}

bool MapTransformer::reachGoal(int id, int step)
{
    double dis = sqrt(pow(paths[id].poses[step].pose.position.x - robot_pos[id].first, 2) + pow(paths[id].poses[step].pose.position.y - robot_pos[id].second, 2));

     if(paths_cpt_idx[id][cpt_ptr[id]] != step && dis <= tolerance_distance){
         ROS_INFO("\033[35mNext normal point is near enough.");
         return true;
     }
     return false;
}

void MapTransformer::tocc(unsigned char order)
{
    toc = ros::Time::now().toSec() - tic;
    if(order == FIRST){
        ROS_INFO("\033[32mOne robot has finished its task. Used time: %lf\n", toc);

        test_data_file.open(data_file_path, ios::in | ios::out | ios::app);
        test_data_file << " First: " << toc;
        test_data_file.close();
    }
    else{
        ROS_INFO("\033[32mFinished all tasks. Total time: %lf\n", toc);

        test_data_file.open(data_file_path, ios::in | ios::out | ios::app);
        test_data_file << " Last: " << toc;
        test_data_file << " ONE_TURN_VAL: " << ONE_TURN_VAL;
        test_data_file.close();
    }
}

void MapTransformer::checkActionState(MoveBaseClient &ac)
{
    actionlib::SimpleClientGoalState state = ac.getState();
     if(state == actionlib::SimpleClientGoalState::SUCCEEDED)      ROS_INFO("Action succeeded!");
     else if(state == actionlib::SimpleClientGoalState::PENDING)   ROS_INFO("Action pending...");
     else if(state == actionlib::SimpleClientGoalState::ACTIVE)    ROS_INFO("Action active!");
     else if(state == actionlib::SimpleClientGoalState::RECALLED)  ROS_INFO("Action recalled!");
     else if(state == actionlib::SimpleClientGoalState::REJECTED)  ROS_INFO("Action rejected.");
     else if(state == actionlib::SimpleClientGoalState::PREEMPTED) ROS_INFO("Action preempted.");
     else if(state == actionlib::SimpleClientGoalState::ABORTED)   ROS_INFO("Action aborted.");
     else if(state == actionlib::SimpleClientGoalState::LOST)      ROS_INFO("Action lost...");
}

Mat MapTransformer::shortenPathsAndGetCheckpoints(Mat &paths_cpt_idx)
{
    Mat paths_idx_tmp(robot_num, vector<int>{});
     for(int i = 0; i < robot_num; ++i){
         paths_idx_tmp[i].push_back(paths_idx[i][0]);
         paths_cpt_idx[i].push_back(0);

         int interval = 4;
         bool cpt_flag = false;
         for(int step = 1; step < paths_idx[i].size() - 1; step++){
             cpt_flag = !isSameLine(paths_idx[i][step - 1], paths_idx[i][step], paths_idx[i][step + 1]);
             if(allocate_method == "MTSP" && mtsp->pt_brickid.count(paths_idx[i][step])){
                 cpt_flag = true;
             }

             if(!cpt_flag && interval){
                 interval--;
             } else {
                 if(cpt_flag)
                     paths_cpt_idx[i].push_back(paths_idx_tmp[i].size());

                 paths_idx_tmp[i].push_back(paths_idx[i][step]);
                 interval = 4;
             }
         }
         paths_cpt_idx[i].push_back(paths_idx_tmp[i].size());
         paths_idx_tmp[i].push_back(paths_idx[i].back());
     }

     return paths_idx_tmp;
}

bool MapTransformer::isSameLine(int &a, int &b, int &c)
{
    return a + c == 2 * b;
}

void MapTransformer::pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &odom, int &id)
{
    //里程计回调函数,用来计算当前机器人位置与前面目标点的距离,判断是否要发新的幕摆点
    if(!initpos_flag[id]){
        initpos_flag[id] = true;
        pose_callback_cnt++;
        printf("pose_callback_cnt:%d\n",pose_callback_cnt);
    }
    //ROS_INFO("get new pose! id is %d", id);
    robot_pos[id].first = odom->pose.pose.position.x;
    robot_pos[id].second = odom->pose.pose.position.y;

    // ROS_INFO("\033[33mRobot %d's current position: %lf %lf", id, robot_pos[id].first, robot_pos[id].second);

    // passed_path.header = poses.header;
    // geometry_msgs::PoseStamped p;
    // p.header = poses.header;
    // p.pose = poses.pose.pose;
    // passed_path.poses.emplace_back(p);
    // pub_passed_path.publish(passed_path);
}

void MapTransformer::getPathInfo(Mat &path)
{
    for (int i = 0; i < path.size(); ++i) {
        int curTurn = 0;
        for (int j = 1; j < path[i].size() - 1; ++j) {
            curTurn += isSameLine(path[i][j - 1], path[i][j], path[i][j + 1]) ? 0 : 1;
        }
        cout << "No." << i << "'s path's length and turns and its value: " << path[i].size() << " " << curTurn << " " << 1.0 * path[i].size() + ONE_TURN_VAL * curTurn << endl;
    }
    cout << "-----------------------------------------\n";
}

void MapTransformer::sendGoals()
{
    goal_ptr.resize(robot_num, 0);
     cpt_ptr.resize(robot_num, 0);
     vector<MoveBaseClient*> ac_ptr(robot_num);

     for(int i = 0; i < robot_num; ++i){
         string ac_topic = "/robot" + std::to_string(i + 1) + "/move_base";
         ac_ptr[i] = new MoveBaseClient(ac_topic, true);
         while(!(*ac_ptr[i]).waitForServer(ros::Duration(5.0))){
             ROS_INFO("Waiting for the move_base_%d action server to come up", i);
         }
     }

     int counter = 0;
     vector<bool> finish_cover(robot_num, false);
     bool finish_task = false;
     int spin_times = 0;     //控制路径发布频率
     ros::Rate r(20);
     while(ros::ok()){
         ros::spinOnce();   // 得先收到机器人初始点位置才行

         if(coverage_action){
             for(int i = 0; i < robot_num; ++i){
                 if(goal_ptr[i] == paths[i].poses.size() && (*ac_ptr[i]).getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
                     if(!counter)    tocc(FIRST);
                     if(!finish_cover[i]){
                         counter++;
                         finish_cover[i] = true;
                     }
                     continue;
                 }

                 // checkActionState(*ac_ptr[i]);
                 // if one robot is near enough(distance less than tolerance), then goes for next goal(reachGoal's work)
                 if(!goal_ptr[i] || reachGoal(i, goal_ptr[i] - 1) || (*ac_ptr[i]).getState() == actionlib::SimpleClientGoalState::SUCCEEDED
                         || (*ac_ptr[i]).getState() == actionlib::SimpleClientGoalState::ABORTED){
                     if(goal_ptr[i] - 1 == paths_cpt_idx[i][cpt_ptr[i]])
                         cpt_ptr[i]++;

                     sendOneGoal(i, goal_ptr[i]++, *ac_ptr[i]);
                 }
             }

             if(!finish_task && counter == robot_num){
                 tocc(LAST);
                 finish_task = true;
             }
         }

         // show plan path
         if(spin_times == 10){
             for(int i = 0; i < robot_num; i++)  path_publishers[i].publish(paths[i]);

             spin_times = 0;
         }

         // TODO: show actual path

         spin_times++;

         r.sleep();
     }
}

bool MapTransformer::createPaths()
{
     // construct robot init pos index vector
     double origin_x = coverage_map.info.origin.position.x;
     double origin_y = coverage_map.info.origin.position.y;
     for(int i = 0; i < robot_num; ++i){
         cout << robot_pos[i].first << " " << robot_pos[i].second << endl;
         int robot_index = (int)((robot_pos[i].first - origin_x) / cmres) + ((int)((robot_pos[i].second - origin_y) / cmres) * cmw);  // 如果地图原点不是(0, 0)，则需要转换时减掉原点xy值
         robot_init_pos.push_back(robot_index);
         cout << "No." << i + 1 << "'s robot initial position index: " << robot_index << endl;
     }

     // construct MST and gain index path
     tic = ros::Time::now().toSec();
//     if(allocate_method == "DARP"){
//         // TODO
//         // Map, robot_pos, robot_num, &coverage_map
//         DARPPlanner *planner = new DARPPlanner(Map, Region, robot_pos, robot_num, &coverage_map);

//         if(MST_shape == "RECT_DIV")             paths_idx = planner->plan("RECT_DIV");
//         else if(MST_shape == "DFS_VERTICAL")    paths_idx = planner->plan("DFS_VERTICAL");
//         else if(MST_shape == "DFS_HORIZONTAL")  paths_idx = planner->plan("DFS_HORIZONTAL");
//         else if(MST_shape == "KRUSKAL")         paths_idx = planner->plan("KRUSKAL");
//         else if(MST_shape == "ACO_OPT")         paths_idx = planner->plan("ACO_OPT");
//         else if(MST_shape == "DINIC")           paths_idx = planner->plan("DINIC");
//         else{
//             ROS_ERROR("Please check shape's name in launch file!");
//             return false;
//         }
//     }
 //        else
     if(allocate_method == "MSTC"){
         if(MST_shape == "DINIC"){
             Dinic *dinic = new Dinic();
             MST = dinic->dinic_solver(Map, true);
             delete  dinic;
         }
         else {
             ONE_TURN_VAL = 0.0;
             Division mstc_div(Map);
             if(MST_shape == "RECT_DIV")             MST = mstc_div.rectDivisionSolver();
             else if(MST_shape == "DFS_VERTICAL")    MST = mstc_div.dfsWithStackSolver(VERTICAL);
             else if(MST_shape == "DFS_HORIZONTAL")  MST = mstc_div.dfsWithStackSolver(HORIZONTAL);
             else if(MST_shape == "BFS_VERTICAL")    MST = mstc_div.bfsSolver(VERTICAL);
             else if(MST_shape == "BFS_HORIZONTAL")  MST = mstc_div.bfsSolver(HORIZONTAL);
             else if(MST_shape == "KRUSKAL")         MST = mstc_div.kruskalSolver();
             else if(MST_shape == "ACO_OPT"){
                 ACO_STC aco(1, 1, 1, 0.15, 60, 300, Map, MST);
                 MST = aco.aco_stc_solver();
             }
             else if(MST_shape == "HEURISTIC"){
                 HeuristicSolver::HeuristicPartition hp(Map, hp_max_iter);
                 MST = hp.hpSolver(true);
             }
             else{
                 ROS_ERROR("Please check shape's name in launch file!");
                 return false;
             }
         }

         ros::NodeHandle nh;
         PathCut cut(Map, Region, MST, robot_init_pos, &nh, boost::make_shared<nav_msgs::OccupancyGrid>(map), useROSPlanner, coverAndReturn);
         paths_idx = cut.cutSolver();
     }
//     else if(allocate_method == "MTSP"){
//         ONE_TURN_VAL = 0.0;

//         // crossover_pb, mutation_pb, elite_rate, sales_men, max_iter, ppl_sz, unchanged_iter;
//         GA_CONF conf{ 0.9, 0.01, 0.3, robot_num, GA_max_iter, 60, unchanged_iter };
//         if(MST_shape == "HEURISTIC"){

//         }
//     else if(MST_shape == "OARP"){
//             cout << Map.size() << ", " << Map[0].size() << "\n";
//             Dinic dinic;
//             dinic.dinic_solver(Map, false);
//             dinic.formBricksForMTSP(Map);
//             mtsp = new MTSP(conf, dinic, robot_init_pos, cmw, coverAndReturn);

//             for(int i = 0; i < GA_max_iter; ++i){
//                 mtsp->GANextGeneration();
//                 if(mtsp->unchanged_gens >= unchanged_iter)   break;
//                 if(i % 1000 == 0){
//                     // display cost for debugging
//                     cout << "GA-cost: " << mtsp->best_val << "\n";
//                 }
//             }

//             paths_idx = mtsp->MTSP2Path();

//             for(int i = 0; i < paths_idx.size(); ++i){
//                 cout << "path " << i << ": ";
//                 for(int j = 0; j < paths_idx[i].size(); ++j){
//                     cout << paths_idx[i][j] << " ";
//                 }
//                 cout << "\n";
//             }
//             // return false;
//         } else {
//             ROS_ERROR("Please check shape's name in launch file!");
//             return false;
//         }
//     } else {
//         ROS_ERROR("Please check allocated method's name in launch file!");
//         return false;
//     }
//     toc = ros::Time::now().toSec();
//     test_data_file.open(data_file_path, ios::in | ios::out | ios::app);
//     test_data_file << " planner used time:" << toc - tic << " ";
//     test_data_file.close();

     getPathInfo(paths_idx);

     // try to reduce some useless points in path in order to acclerate
     paths_cpt_idx.resize(robot_num);
     paths_idx = shortenPathsAndGetCheckpoints(paths_cpt_idx);

     cout << "get every path pose stamps...\n";
     // 将index paths转化为一个个位姿
     paths.resize(paths_idx.size());
     for(int i = 0; i < paths_idx.size(); ++i){
         paths[i].header.frame_id = "map";
         for(int j = 0; j < paths_idx[i].size(); ++j){
             int dy = paths_idx[i][j] / cmw;
             int dx = paths_idx[i][j] % cmw;

             // 判断每个goal的yaw
             double yaw = 0;
             if(j < paths_idx[i].size() - 1){
                 int dx1 = paths_idx[i][j + 1] % cmw;
                 int dy1 = paths_idx[i][j + 1] / cmw;

                 yaw = atan2(1.0 * (dy1 - dy), 1.0 * (dx1 - dx));
             }
             // yaw转四元数
             paths[i].poses.push_back({});
             paths[i].poses.back().pose.position.x = dx * cmres + 0.25;
             paths[i].poses.back().pose.position.y = dy * cmres + 0.25;
             paths[i].poses.back().pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
         }
     }

      getPathInfo(paths_idx);
//      test : visualize paths in rviz
//      while(ros::ok()){
//         for(int i = 0; i < robot_num; i++)
//             path_publishers[i].publish(paths[i]);
//      }

     return true;
}

void MapTransformer::run()
{
    // initialize Map and Region(resize and fill in)
    Map.resize(cmh / 2, vector<int>(cmw / 2, 0));
    Region.resize(cmh, vector<int>(cmw, 0));

    // 去掉地图上的孤岛 一次BFS即可
    eliminateIslands();
    showMapAndRegionInf();


    if(!createPaths())
    {
        return;
    }

    emit emitPath(paths);
}
